#include "clustertype.h"
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <std_msgs/UInt8MultiArray.h>

static Cluster_Context g_stClusterCntxt;
/*************************************cluster_ctrl.cpp****************************************/
Cluster_Context *fnCluster_ctrl_GetClusterCntxt(void)
{
    return &g_stClusterCntxt;
}
/*************************************cluster_ctrl.cpp****************************************/
void *fnCluster_buf_AllocBuf(cUint32 uSize)
{
    if(uSize > 0)
    {
        return (void *)malloc(uSize);
    }
    else {
        return nullptr;
    }
}

bool fnCluster_buf_FreeBuf(cUint8 *pBuf)
{
    if(nullptr != pBuf)
    {
        free(pBuf);
    }
    else {
        return false;
    }
    return true;
}

/*************************************cluster_codec.cpp****************************************/
void fnCluster_codec_GpsConvertToEnu(cFloat64 &enuX, cFloat64 &enuY, cFloat64 tarLon, cFloat64 tarLat, cFloat64 oriLon, cFloat64 oriLat)
{
    cFloat64 earthLen = 6378137.0;
    //float64 pi = 3.141592653589793;
    cFloat64 degToRad = 3.141592653589793 / 180;
    cFloat64 deltaLon = tarLon - oriLon;
    cFloat64 deltaLat = tarLat - oriLat;

    enuY = (deltaLat * degToRad) * earthLen;
    enuX = (deltaLon * degToRad) * earthLen * cos(tarLat * degToRad);
}

void fnCluster_codec_StreamToEightByte(cUint8 *pInMsgStream, cUint64 *puDestVal)
{
    cUint64 uIntTempToFloat = 0;
    uIntTempToFloat = pInMsgStream[0];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[1];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[2];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[3];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[4];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[5];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[6];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[7];
    *puDestVal = uIntTempToFloat;
}

void fnCluster_codec_StreamToFourByte(cUint8 *pInMsgStream, cUint32 *puDestVal)
{
    cUint32 uIntTempToFloat = 0;
    uIntTempToFloat = pInMsgStream[0];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[1];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[2];
    uIntTempToFloat = (uIntTempToFloat << 8) | pInMsgStream[3];
    *puDestVal = uIntTempToFloat;
}

bool fnCluster_codec_DecodeControlMsg(cUint8 *pInMsgStream, Cluster_Control_Message *pstMainNodeMsg, cUint32 *pstStreamIndex)
{
    cUint32 uMsgLen = 2;
    cUint32 uSubIndex = 0;
    cUint32 uSubMsgLen = 0;
    cUint32 uStreamIndex = 0;
    cUint32 uSubNodeMapID = 0;
    cUint32 auSubNodeMapID[CLUSTER_MAX_NODE_NUM];
    Cluster_Context *pstClusterContext = fnCluster_ctrl_GetClusterCntxt();
    cUint32 uSubNodeNum = pstClusterContext->uSubNodeNum;
    memcpy(auSubNodeMapID, pstClusterContext->auSubNodeMapID, sizeof(cUint32) * CLUSTER_MAX_NODE_NUM);

    uMsgLen += (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
    uStreamIndex += 2;
    pstMainNodeMsg->stMsgHeader.uRecvID = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
    uStreamIndex += 2;
    pstMainNodeMsg->stMsgHeader.uSelectIDNum = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
    uStreamIndex += 2;
    for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
    {
        pstMainNodeMsg->stMsgHeader.auSelectID[i] = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        uStreamIndex += 2;
    }
    pstMainNodeMsg->stMsgHeader.enClusterCmd = (Cluster_Command)pInMsgStream[uStreamIndex++];

    pstMainNodeMsg->uEncodeDirectField = 0;

    if((uStreamIndex + 2 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_SUBNODE_MAP_ID) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstMainNodeMsg->uEncodeDirectField |= (cUint8)(CLUSTER_MAINNODE_MSG_IE_SUBNODE_MAP_ID);
        pstMainNodeMsg->uSubNodeNumber = (cUint32)(pInMsgStream[uStreamIndex++] & 0xFF);
        for(cUint32 i = 0; i < pstMainNodeMsg->uSubNodeNumber; i++)
        {
            pstMainNodeMsg->auSubNodeMapID[i] = (cUint32)(pInMsgStream[uStreamIndex++] & 0xFF);
        }
        uSubNodeNum = pstMainNodeMsg->uSubNodeNumber;
        memcpy(auSubNodeMapID, pstMainNodeMsg->auSubNodeMapID, sizeof(cUint32) * CLUSTER_MAX_NODE_NUM);
    }

    if((uStreamIndex + 3 < uMsgLen) && (((cUint8)(CLUSTER_OPTIONAL_IE_PATH_POINT) == pInMsgStream[uStreamIndex]) ||
        ((cUint8)(CLUSTER_OPTIONAL_IE_PATH_POINT_GPS) == pInMsgStream[uStreamIndex])))
    {
        if((cUint8)(CLUSTER_OPTIONAL_IE_PATH_POINT) == pInMsgStream[uStreamIndex])
        {
            pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PATH_POINT;
        }
        else
        {
            pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PATH_POINT_GPS;
        }

        uStreamIndex++;
        uSubMsgLen = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        uStreamIndex += 2;
        if((uStreamIndex + uSubMsgLen) > uMsgLen)
        {
            goto DECODE_FAIL;
        }

        uSubIndex = 0;
        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            for(cUint32 i = 0; i < uSubNodeNum; i++)
            {
                uSubNodeMapID = auSubNodeMapID[i];
                pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID] = (Cluster_Path_Buf *)fnCluster_buf_AllocBuf(sizeof(Cluster_Path_Buf));
                if(NULL == pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID])
                {
                    CLUSTER_PRINT("pastSubNodePathPoint is NULL!");
                }

                pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID]->uPathNum = (cUint32)(pInMsgStream[uStreamIndex + uSubIndex++] & 0xFF);
                if((uSubIndex + pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID]->uPathNum * 3 * 8) > uSubMsgLen)
                {
                    goto DECODE_FAIL;
                }

                for(cUint32 j = 0; j < pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID]->uPathNum; j++)
                {
                    for(int k = 0; k < 3; k++)
                    {
                        fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex + uSubIndex], (cUint64 *)&pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID]->aPathPointBuf[j].aArray[k]);
                        uSubIndex += 8;
                    }
                }
            }
        }
        else if(CLUSTER_RECV_ID_ALL == pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            pstMainNodeMsg->pastSubNodePathPoint[0] = (Cluster_Path_Buf *)fnCluster_buf_AllocBuf(sizeof(Cluster_Path_Buf));
            if(NULL == pstMainNodeMsg->pastSubNodePathPoint[0])
            {
                CLUSTER_PRINT("pastSubNodePathPoint is NULL!");
            }
            pstMainNodeMsg->pastSubNodePathPoint[0]->uPathNum = (cUint32)(pInMsgStream[uStreamIndex + uSubIndex++] & 0xFF);
            if((pstMainNodeMsg->pastSubNodePathPoint[0]->uPathNum * 3 * 8 + 1) > uSubMsgLen)
            {
                goto DECODE_FAIL;
            }
            for(cUint32 j = 0; j < pstMainNodeMsg->pastSubNodePathPoint[0]->uPathNum; j++)
            {
                for(int k = 0; k < 3; k++)
                {
                    fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex + uSubIndex], (cUint64 *)&pstMainNodeMsg->pastSubNodePathPoint[0]->aPathPointBuf[j].aArray[k]);
                    uSubIndex += 8;
                }
            }
        }
        else
        {
            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID] = (Cluster_Path_Buf *)fnCluster_buf_AllocBuf(sizeof(Cluster_Path_Buf));
                if(NULL == pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID])
                {
                    CLUSTER_PRINT("pastSubNodePathPoint is NULL!");
                }

                pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID]->uPathNum = (cUint32)(pInMsgStream[uStreamIndex + uSubIndex++] & 0xFF);
                if((uSubIndex + pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID]->uPathNum * 3 * 8) > uSubMsgLen)
                {
                    goto DECODE_FAIL;
                }
                for(cUint32 j = 0; j < pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID]->uPathNum; j++)
                {
                    for(int k = 0; k < 3; k++)
                    {
                        fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex + uSubIndex], (cUint64 *)&pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID]->aPathPointBuf[j].aArray[k]);
                        uSubIndex += 8;
                    }
                }
            }
        }
        uStreamIndex += uSubMsgLen;
    }

    if((uStreamIndex + 2 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_FORMATION_POS) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_FORMATION_POS;
        uSubMsgLen = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        uStreamIndex += 2;

        if((uStreamIndex + uSubMsgLen) > uMsgLen)
        {
            goto DECODE_FAIL;
        }

        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            if(uSubMsgLen < uSubNodeNum * 24)
            {
                goto DECODE_FAIL;
            }

            for(cUint32 i = 0; i < uSubNodeNum; i++)
            {
                uSubNodeMapID = auSubNodeMapID[i];
                for(int j = 0; j < 3; j++)
                {
                    fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstMainNodeMsg->astFormationPos[uSubNodeMapID].aArray[j]);
                    uStreamIndex += 8;
                }
            }
        }
        else if(CLUSTER_RECV_ID_ALL == pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            /*printf error!*/
        }
        else
        {
            if(uSubMsgLen < pstMainNodeMsg->stMsgHeader.uSelectIDNum * 24)
            {
                goto DECODE_FAIL;
            }

            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                for(int j = 0; j < 3; j++)
                {
                    fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstMainNodeMsg->astFormationPos[uSubNodeMapID].aArray[j]);
                    uStreamIndex += 8;
                }
            }
        }
    }

    if((uStreamIndex + 2 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_TARGET_ID) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_ASSIGN_TARGET_ID;
        uSubMsgLen = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        uStreamIndex += 2;

        if((uStreamIndex + uSubMsgLen) > uMsgLen)
        {
            goto DECODE_FAIL;
        }

        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            uSubIndex = 0;
            for(int i = 0; uSubMsgLen >= uSubIndex + 4; i++)
            {
                uSubNodeMapID = auSubNodeMapID[i];
                fnCluster_codec_StreamToFourByte(&pInMsgStream[uStreamIndex + uSubIndex], (cUint32 *)&pstMainNodeMsg->auSubNodeAssignTargetID[uSubNodeMapID]);
                uSubIndex += 4;
            }
            uStreamIndex += uSubIndex;
        }
        else if(CLUSTER_RECV_ID_ALL == pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            /*printf error!*/
        }
        else
        {
            if(uSubMsgLen < pstMainNodeMsg->stMsgHeader.uSelectIDNum * 4)
            {
                goto DECODE_FAIL;
            }
            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                fnCluster_codec_StreamToFourByte(&pInMsgStream[uStreamIndex], (cUint32 *)&pstMainNodeMsg->auSubNodeAssignTargetID[uSubNodeMapID]);
                uStreamIndex += 4;
            }
        }
    }

    if((uStreamIndex + 2 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_TARGET_INFO) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_TARGET_POS_ALL;
        //uSubMsgLen = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        pstMainNodeMsg->stTargetGlobalPos.uPathNum = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        uStreamIndex += 2;

        if((uStreamIndex + pstMainNodeMsg->stTargetGlobalPos.uPathNum * 24) > uMsgLen)
        {
            goto DECODE_FAIL;
        }

        //uSubIndex = 0;
        for(cUint32 i = 0; i < pstMainNodeMsg->stTargetGlobalPos.uPathNum; i++)
        {
            for(int j = 0; j < 3; j++)
            {
                fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstMainNodeMsg->stTargetGlobalPos.aPathPointBuf[i].aArray[j]);
                uStreamIndex += 8;
            }
        }
        //uStreamIndex += uSubIndex;
    }

    if((uStreamIndex + 4 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_COMMON_PATH_INDEX) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_COMMON_PATH_INDEX;
        fnCluster_codec_StreamToFourByte(&pInMsgStream[uStreamIndex], (cUint32 *)&pstMainNodeMsg->uCurrExecPathPointIndex);
        uStreamIndex += 4;
    }

    if((uStreamIndex + 1 < uMsgLen) && (cUint8)(CLUSTER_OPTIONAL_IE_FORMATION_TYPE) == pInMsgStream[uStreamIndex])
    {
        uStreamIndex++;
        pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_FORMATIPN_TYPE;

        pstMainNodeMsg->enFormationType = (Cluster_Formation_Type)pInMsgStream[uStreamIndex++];
    }

    if((uStreamIndex < uMsgLen) && (((cUint8)(CLUSTER_OPTIONAL_IE_SCAN_AREA) == pInMsgStream[uStreamIndex]) ||
        ((cUint8)(CLUSTER_OPTIONAL_IE_SCAN_AREA_GPS) == pInMsgStream[uStreamIndex])))
    {
        if((cUint8)(CLUSTER_OPTIONAL_IE_SCAN_AREA) == pInMsgStream[uStreamIndex])
        {
            pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_AREA_INFO;
        }
        else
        {
            pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_AREA_INFO_GPS;
        }

        uStreamIndex++;
        pstMainNodeMsg->stScanArea.uPathNum = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        uStreamIndex += 2;

        if((uStreamIndex + 8 * 3 * pstMainNodeMsg->stScanArea.uPathNum) <= uMsgLen)
        {
            for(cUint32 i = 0; i < pstMainNodeMsg->stScanArea.uPathNum; i++)
            {
                for(int j = 0; j < 3; j++)
                {
                    fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstMainNodeMsg->stScanArea.aPathPointBuf[i].aArray[j]);
                    uStreamIndex += 8;
                }
            }
        }
        else
        {
            return false;
        }
    }

    if((uStreamIndex < uMsgLen) && (((cUint8)(CLUSTER_OPTIONAL_IE_PICKUP_POINT) == pInMsgStream[uStreamIndex]) ||
        ((cUint8)(CLUSTER_OPTIONAL_IE_PICKUP_POINT_GPS) == pInMsgStream[uStreamIndex])))
    {
        if((cUint8)(CLUSTER_OPTIONAL_IE_PICKUP_POINT) == pInMsgStream[uStreamIndex])
        {
            pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT;
        }
        else
        {
            pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT_GPS;
        }

        uStreamIndex++;
        if((uStreamIndex + 8 * 3) <= uMsgLen)
        {
            for(int i = 0; i < 3; i++)
            {
                fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstMainNodeMsg->stPickupPoint.aArray[i]);
                uStreamIndex += 8;
            }
        }
        else
        {
            return false;
        }
    }

    if((uStreamIndex + 4 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_VELOCITY_CMD) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstMainNodeMsg->uEncodeDirectField |= CLUSTER_MAINNODE_MSG_IE_VELOCITY;

        fnCluster_codec_StreamToFourByte(&pInMsgStream[uStreamIndex], (cUint32 *)&pstMainNodeMsg->velocityLimit);
        uStreamIndex += 4;

    }

    if(uMsgLen != uStreamIndex)/*解码过程只读取不写入，即使越界不会破环系统，所以溢出判断放到最后*/
    {
DECODE_FAIL:
        for(int i = 0; i < CLUSTER_MAX_NODE_NUM; i++)
        {
            if(NULL != pstMainNodeMsg->pastSubNodePathPoint[i])
            {
                fnCluster_buf_FreeBuf((cUint8 *)pstMainNodeMsg->pastSubNodePathPoint[i]);
                pstMainNodeMsg->pastSubNodePathPoint[i] = NULL;
            }
        }
        return false;
    }

    *pstStreamIndex += uStreamIndex;
    return true;
}

bool fnCluster_codec_DecodeStateMsg(cUint8 *pInMsgStream, Cluster_State_Message *pstSubNodeMsg, cUint32 *pstStreamIndex)
{
    cUint32 uMsgLen = 2;
    cUint32 uSubIndex = 0;
    cUint32 uSubMsgLen = 0;
    cUint32 uStreamIndex = 0;

    uMsgLen += (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
    uStreamIndex += 2;
    pstSubNodeMsg->stMsgHeader.uRecvID = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
    uStreamIndex += 2;
    pstSubNodeMsg->stMsgHeader.uSelectIDNum = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
    uStreamIndex += 2;
    for(cUint32 i = 0; i < pstSubNodeMsg->stMsgHeader.uSelectIDNum; i++)
    {
        pstSubNodeMsg->stMsgHeader.auSelectID[i] = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        uStreamIndex += 2;
    }
    pstSubNodeMsg->stMsgHeader.enClusterCmd = (Cluster_Command)pInMsgStream[uStreamIndex++];

    pstSubNodeMsg->uEncodeDirectField = 0;

    if((uStreamIndex < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_INITIAL_POS) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_INITIAL_POS;

        if((uStreamIndex + 8 * 3) <= uMsgLen)
        {
            for(int i = 0; i < 3; i++)
            {
                fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stSubNodeInitialPos.aArray[i]);
                uStreamIndex += 8;
            }
        }
        else
        {
            return false;
        }
    }

    if((uStreamIndex < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_OFFSET_POS) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_OFFSET_POS;

        if((uStreamIndex + 8 * 3) <= uMsgLen)
        {
            for(int i = 0; i < 3; i++)
            {
                fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stSubNodeOffsetPos.aArray[i]);
                uStreamIndex += 8;
            }
        }
        else
        {
            return false;
        }
    }

    if((uStreamIndex < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_GLOBAL_POS) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_GLOBAL_POS;

        if((uStreamIndex + 8 * 3) <= uMsgLen)
        {
            for(int i = 0; i < 3; i++)
            {
                fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stSubNodeGlobalPos.aArray[i]);
                uStreamIndex += 8;
            }
        }
        else
        {
            return false;
        }
    }

    if((uStreamIndex < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_MEMBER_VEL) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_VELOCITY;

        if((uStreamIndex + 8 * 3) <= uMsgLen)
        {
            for(int i = 0; i < 3; i++)
            {
                fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stSubNodeVelocity.aArray[i]);
                uStreamIndex += 8;
            }
        }
        else
        {
            return false;
        }
    }

    if((uStreamIndex + 1 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_NETWORK_STATE) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_NETWORK_STATE;

        pstSubNodeMsg->enClusterNetworkState = (Cluster_Network_State)pInMsgStream[uStreamIndex++];
    }

    if((uStreamIndex + 1 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_SUBNODE_STATE) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_SUBNODE_STATE;

        pstSubNodeMsg->enClusterSubNodeState = (Cluster_SubNode_State)pInMsgStream[uStreamIndex++];
    }

    if((uStreamIndex + 1 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_MISSION_STATE) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_MISSION_STATE;

        pstSubNodeMsg->enClusterMissionState = (Cluster_Mission_State)pInMsgStream[uStreamIndex++];
    }

    if((uStreamIndex + 1 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_WORK_STATE) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_WORK_STATE;

        pstSubNodeMsg->enClusterWorkState = (Cluster_Work_State)pInMsgStream[uStreamIndex++];
    }

    if((uStreamIndex + 2 < uMsgLen) && ((cUint8)(CLUSTER_OPTIONAL_IE_TARGET_INFO) == pInMsgStream[uStreamIndex]))
    {
        uStreamIndex++;
        pstSubNodeMsg->uEncodeDirectField |= CLUSTER_SUBNODE_MSG_IE_TARGET_POS;
        //uSubMsgLen = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        pstSubNodeMsg->stTargetGlobalPos.uPathNum = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
        uStreamIndex += 2;

        if((uStreamIndex + pstSubNodeMsg->stTargetGlobalPos.uPathNum * 24) > uMsgLen)
        {
            return false;
        }

        //uSubIndex = 0;
        for(cUint32 i = 0; i < pstSubNodeMsg->stTargetGlobalPos.uPathNum; i++)
        {
            for(int j = 0; j < 3; j++)
            {
                fnCluster_codec_StreamToEightByte(&pInMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stTargetGlobalPos.aPathPointBuf[i].aArray[j]);
                uStreamIndex += 8;
            }
        }
        //uStreamIndex += uSubIndex;
    }

    if(uMsgLen != uStreamIndex)/*解码过程只读取不写入，即使越界不会破环系统，所以溢出判断放到最后*/
    {
        return false;
    }

    *pstStreamIndex += uStreamIndex;
    return true;
}

void fnCluster_codec_DecodeMessage(cUint8 *pInMsgStream, cUint32 uInMsgLen, Cluster_Message_Info *pstDecodeMsg)
{
    cUint32 uMsgLen = 0;
    cUint32 uStreamIndex = 0;
    cUint32 uClusterCmdIndex = 0;
    Cluster_Message_Content *puMsgBuf = NULL;
    Cluster_Message_Header stMsgHeader;

    if((NULL == pInMsgStream) || (NULL == pstDecodeMsg))
    {
        CLUSTER_PRINT("InputParam Error!");
    }

//    stMsgHeader.uSN = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
//    uStreamIndex += 2;
    uStreamIndex += 4;

    stMsgHeader.uSendID = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
    uStreamIndex += 2;
    stMsgHeader.uMainNodeID = (cUint32)((pInMsgStream[uStreamIndex] << 8) | pInMsgStream[uStreamIndex + 1]) & 0xFFFF;
    uStreamIndex += 2;

    while(uInMsgLen > uStreamIndex)
    {
        //pstDecodeMsg->aenClusterCmd[uClusterCmdIndex] = (Cluster_Command)pInMsgStream[uStreamIndex + 6];
        pstDecodeMsg->aenClusterMsgType[uClusterCmdIndex] = (Cluster_Message_Type)pInMsgStream[uStreamIndex++];
        pstDecodeMsg->pastMessageCntnt[uClusterCmdIndex] = (Cluster_Message_Content *)fnCluster_buf_AllocBuf(sizeof(Cluster_Message_Content));
        puMsgBuf = (Cluster_Message_Content *)pstDecodeMsg->pastMessageCntnt[uClusterCmdIndex];

        if(NULL == puMsgBuf)
        {
            CLUSTER_PRINT("puMsgBuf is NULL!");
        }

        switch (pstDecodeMsg->aenClusterMsgType[uClusterCmdIndex])
        {
        case    CLUSTER_MSG_TYPE_STATE:
            {
                puMsgBuf->stStateMessage.stMsgHeader = stMsgHeader;
                if(!fnCluster_codec_DecodeStateMsg(&pInMsgStream[uStreamIndex], &puMsgBuf->stStateMessage, &uStreamIndex))
                {
                    CLUSTER_PRINT("Decode SubNodeMsg Error!");
                }
            }
        break;
        case    CLUSTER_MSG_TYPE_CONTROL:
            {
                puMsgBuf->stControlMessage.stMsgHeader = stMsgHeader;
                if(!fnCluster_codec_DecodeControlMsg(&pInMsgStream[uStreamIndex], &puMsgBuf->stControlMessage, &uStreamIndex))
                {
                    CLUSTER_PRINT("Decode MainNodeMsg Error!");
                }
            }
        break;
        default:
            {/*do Nothing*/}
        break;
        }

        uClusterCmdIndex++;
    }
}

void fnCluster_codec_EightByteToStream(cUint8 *pOutMsgStream, cUint64 *puSourceVal)
{
    cUint64 uFloatTempToInt = *puSourceVal;
    pOutMsgStream[0] = (cUint8)(uFloatTempToInt >> 56);
    pOutMsgStream[1] = (cUint8)(uFloatTempToInt >> 48);
    pOutMsgStream[2] = (cUint8)(uFloatTempToInt >> 40);
    pOutMsgStream[3] = (cUint8)(uFloatTempToInt >> 32);
    pOutMsgStream[4] = (cUint8)(uFloatTempToInt >> 24);
    pOutMsgStream[5] = (cUint8)(uFloatTempToInt >> 16);
    pOutMsgStream[6] = (cUint8)(uFloatTempToInt >> 8);
    pOutMsgStream[7] = (cUint8)(uFloatTempToInt);
}

void fnCluster_codec_FourByteToStream(cUint8 *pOutMsgStream, cUint32 *puSourceVal)
{
    cUint32 uFloatTempToInt = *puSourceVal;
    pOutMsgStream[0] = (cUint8)(uFloatTempToInt >> 24);
    pOutMsgStream[1] = (cUint8)(uFloatTempToInt >> 16);
    pOutMsgStream[2] = (cUint8)(uFloatTempToInt >> 8);
    pOutMsgStream[3] = (cUint8)(uFloatTempToInt);
}

void fnCluster_codec_EncodeControlMsg(cUint8 *pOutMsgStream, Cluster_Control_Message *pstMainNodeMsg, cUint32 *pstStreamIndex)
{
    cUint32 uSubIndex = 0;
    cUint32 uStreamIndex = 2;
    cUint32 uSubNodeMapID = 0;
    Cluster_Context *pstClusterContext = fnCluster_ctrl_GetClusterCntxt();

    Cluster_Path_Buf *pstPathPointBuf;
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stMsgHeader.uRecvID >> 8);
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stMsgHeader.uRecvID);
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stMsgHeader.uSelectIDNum >> 8);
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stMsgHeader.uSelectIDNum);
    for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stMsgHeader.auSelectID[i] >> 8);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stMsgHeader.auSelectID[i]);
    }
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stMsgHeader.enClusterCmd);

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_SUBNODE_MAP_ID))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_SUBNODE_MAP_ID);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->uSubNodeNumber);
        for(cUint32 i = 0; i < pstMainNodeMsg->uSubNodeNumber; i++)
        {
            pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->auSubNodeMapID[i]);
        }
    }

    if((0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PATH_POINT)) ||
        (0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PATH_POINT_GPS)))
    {
        if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PATH_POINT))
        {
            pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_PATH_POINT);
        }
        else
        {
            pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_PATH_POINT_GPS);
        }

        uSubIndex = 0;
        uStreamIndex += 2;
        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            for(cUint32 i = 0; i < pstClusterContext->uSubNodeNum; i++)
            {
                uSubNodeMapID = pstClusterContext->auSubNodeMapID[i];
                pstPathPointBuf = pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID];
                pOutMsgStream[uStreamIndex + uSubIndex++] = (cUint8)(pstPathPointBuf->uPathNum);
                for(cUint32 j = 0; j < pstPathPointBuf->uPathNum; j++)
                {
                    for(int k = 0; k < 3; k++)
                    {
                        fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex + uSubIndex], (cUint64 *)&pstPathPointBuf->aPathPointBuf[j].aArray[k]);
                        uSubIndex += 8;
                    }
                }
            }
        }
        else if(CLUSTER_RECV_ID_ALL == pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            pstPathPointBuf = pstMainNodeMsg->pastSubNodePathPoint[0];
            pOutMsgStream[uStreamIndex + uSubIndex++] = (cUint8)(pstPathPointBuf->uPathNum);
            for(cUint32 j = 0; j < pstPathPointBuf->uPathNum; j++)
            {
                for(int k = 0; k < 3; k++)
                {
                    fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex + uSubIndex], (cUint64 *)&pstPathPointBuf->aPathPointBuf[j].aArray[k]);
                    uSubIndex += 8;
                }
            }
        }
        else
        {
            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                pstPathPointBuf = pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID];
                pOutMsgStream[uStreamIndex + uSubIndex++] = (cUint8)(pstPathPointBuf->uPathNum);
                for(cUint32 j = 0; j < pstPathPointBuf->uPathNum; j++)
                {
                    for(cUint32 k = 0; k < 3; k++)
                    {
                        fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex + uSubIndex], (cUint64 *)&pstPathPointBuf->aPathPointBuf[j].aArray[k]);
                        uSubIndex += 8;
                    }
                }
            }
        }
        pOutMsgStream[uStreamIndex - 2] = (cUint8)(uSubIndex >> 8);
        pOutMsgStream[uStreamIndex - 1] = (cUint8)(uSubIndex);
        uStreamIndex += uSubIndex;
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_FORMATION_POS))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_FORMATION_POS);

        uSubIndex = 0;
        uStreamIndex += 2;
        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            for(cUint32 i = 0; i < pstClusterContext->uSubNodeNum; i++)
            {
                uSubNodeMapID = pstClusterContext->auSubNodeMapID[i];
                for(int j = 0; j < 3; j++)
                {
                    fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex + uSubIndex], (cUint64 *)&pstMainNodeMsg->astFormationPos[uSubNodeMapID].aArray[j]);
                    uSubIndex += 8;
                }
            }
        }
        else if(CLUSTER_RECV_ID_ALL == pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            /*printf error!*/
        }
        else
        {
            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                for(int j = 0; j < 3; j++)
                {
                    fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex + uSubIndex], (cUint64 *)&pstMainNodeMsg->astFormationPos[uSubNodeMapID].aArray[j]);
                    uSubIndex += 8;
                }
            }
        }
        pOutMsgStream[uStreamIndex - 2] = (cUint8)(uSubIndex >> 8);
        pOutMsgStream[uStreamIndex - 1] = (cUint8)(uSubIndex);
        uStreamIndex += uSubIndex;
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_ASSIGN_TARGET_ID))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_TARGET_ID);

        uSubIndex = 0;
        uStreamIndex += 2;
        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            for(cUint32 i = 0; i < pstClusterContext->uSubNodeNum; i++)
            {
                uSubNodeMapID = pstClusterContext->auSubNodeMapID[i];
                fnCluster_codec_FourByteToStream(&pOutMsgStream[uStreamIndex + uSubIndex], (cUint32 *)&pstMainNodeMsg->auSubNodeAssignTargetID[uSubNodeMapID]);
                uSubIndex += 4;
            }
        }
        else if(CLUSTER_RECV_ID_ALL == pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            /*printf error!*/
        }
        else
        {
            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                fnCluster_codec_FourByteToStream(&pOutMsgStream[uStreamIndex + uSubIndex], (cUint32 *)&pstMainNodeMsg->auSubNodeAssignTargetID[uSubNodeMapID]);
                uSubIndex += 4;
            }
        }
        pOutMsgStream[uStreamIndex - 2] = (cUint8)(uSubIndex >> 8);
        pOutMsgStream[uStreamIndex - 1] = (cUint8)(uSubIndex);
        uStreamIndex += uSubIndex;
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_TARGET_POS_ALL))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_TARGET_INFO);
        //uSubIndex = 0;
        //uStreamIndex += 2;
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stTargetGlobalPos.uPathNum >> 8);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stTargetGlobalPos.uPathNum);
        for(cUint32 i = 0; i < pstMainNodeMsg->stTargetGlobalPos.uPathNum; i++)
        {
            for(int j = 0; j < 3; j++)
            {
                fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex], (cUint64 *)&pstMainNodeMsg->stTargetGlobalPos.aPathPointBuf[i].aArray[j]);
                uStreamIndex += 8;
            }
        }
        //pOutMsgStream[uStreamIndex - 2] = (cUint8)(uSubIndex >> 8);
        //pOutMsgStream[uStreamIndex - 1] = (cUint8)(uSubIndex);
        //uStreamIndex += uSubIndex;
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_COMMON_PATH_INDEX))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_COMMON_PATH_INDEX);
        fnCluster_codec_FourByteToStream(&pOutMsgStream[uStreamIndex], (cUint32 *)&pstMainNodeMsg->uCurrExecPathPointIndex);
        uStreamIndex += 4;
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_FORMATIPN_TYPE))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_FORMATION_TYPE);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->enFormationType);
    }

    if((0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_AREA_INFO)) ||
        (0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_AREA_INFO_GPS)))
    {
        if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_AREA_INFO))
        {
            pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_SCAN_AREA);
        }
        else
        {
            pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_SCAN_AREA_GPS);
        }

        pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stScanArea.uPathNum >> 8);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstMainNodeMsg->stScanArea.uPathNum);
        for(cUint32 i = 0; i < pstMainNodeMsg->stScanArea.uPathNum; i++)
        {
            for(int j = 0; j < 3; j++)
            {
                fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex], (cUint64 *)&pstMainNodeMsg->stScanArea.aPathPointBuf[i].aArray[j]);
                uStreamIndex += 8;
            }
        }
    }

    if((0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT)) ||
        (0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT_GPS)))
    {
        if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT))
        {
            pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_PICKUP_POINT);
        }
        else
        {
            pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_PICKUP_POINT_GPS);
        }

        for(int i = 0; i < 3; i++)
        {
            fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex], (cUint64 *)&pstMainNodeMsg->stPickupPoint.aArray[i]);
            uStreamIndex += 8;
        }
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_VELOCITY))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_VELOCITY_CMD);
        fnCluster_codec_FourByteToStream(&pOutMsgStream[uStreamIndex], (cUint32 *)&pstMainNodeMsg->velocityLimit);
        uStreamIndex += 4;
    }

    pOutMsgStream[0] = (cUint8)((uStreamIndex - 2) >> 8);
    pOutMsgStream[1] = (cUint8)(uStreamIndex - 2);
    *pstStreamIndex += uStreamIndex;
}

void fnCluster_codec_EncodeStateMsg(cUint8 *pOutMsgStream, Cluster_State_Message *pstSubNodeMsg, cUint32 *pstStreamIndex)
{
    cUint32 uSubIndex = 0;
    cUint32 uStreamIndex = 2;
    Cluster_Context *pstClusterContext = fnCluster_ctrl_GetClusterCntxt();

    pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stMsgHeader.uRecvID >> 8);
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stMsgHeader.uRecvID);
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stMsgHeader.uSelectIDNum >> 8);
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stMsgHeader.uSelectIDNum);
    for(cUint32 i = 0; i < pstSubNodeMsg->stMsgHeader.uSelectIDNum; i++)
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stMsgHeader.auSelectID[i] >> 8);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stMsgHeader.auSelectID[i]);
    }
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stMsgHeader.enClusterCmd);

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_INITIAL_POS))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_INITIAL_POS);
        for(int i = 0; i < 3; i++)
        {
            fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stSubNodeInitialPos.aArray[i]);
            uStreamIndex += 8;
        }
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_OFFSET_POS))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_OFFSET_POS);
        for(int i = 0; i < 3; i++)
        {
            fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stSubNodeOffsetPos.aArray[i]);
            uStreamIndex += 8;
        }
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_GLOBAL_POS))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_GLOBAL_POS);
        for(int i = 0; i < 3; i++)
        {
            fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stSubNodeGlobalPos.aArray[i]);
            uStreamIndex += 8;
        }
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_VELOCITY))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_MEMBER_VEL);
        for(int i = 0; i < 3; i++)
        {
            fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stSubNodeVelocity.aArray[i]);
            uStreamIndex += 8;
        }
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_NETWORK_STATE))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_NETWORK_STATE);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->enClusterNetworkState);
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_SUBNODE_STATE))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_SUBNODE_STATE);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->enClusterSubNodeState);
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_MISSION_STATE))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_MISSION_STATE);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->enClusterMissionState);
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_WORK_STATE))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_WORK_STATE);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->enClusterWorkState);
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_TARGET_POS))
    {
        pOutMsgStream[uStreamIndex++] = (cUint8)(CLUSTER_OPTIONAL_IE_TARGET_INFO);
        //uSubIndex = 0;
        //uStreamIndex += 2;
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stTargetGlobalPos.uPathNum >> 8);
        pOutMsgStream[uStreamIndex++] = (cUint8)(pstSubNodeMsg->stTargetGlobalPos.uPathNum);
        for(cUint32 i = 0; i < pstSubNodeMsg->stTargetGlobalPos.uPathNum; i++)
        {
            for(int j = 0; j < 3; j++)
            {
                fnCluster_codec_EightByteToStream(&pOutMsgStream[uStreamIndex], (cUint64 *)&pstSubNodeMsg->stTargetGlobalPos.aPathPointBuf[i].aArray[j]);
                uStreamIndex += 8;
            }
        }
        //pOutMsgStream[uStreamIndex - 2] = (cUint8)(uSubIndex >> 8);
        //pOutMsgStream[uStreamIndex - 1] = (cUint8)(uSubIndex);
        //uStreamIndex += uSubIndex;
    }

    pOutMsgStream[0] = (cUint8)((uStreamIndex - 2) >> 8);
    pOutMsgStream[1] = (cUint8)(uStreamIndex - 2);
    *pstStreamIndex += uStreamIndex;
}

cUint32 fnCluster_codec_EncodeMessage(cUint8 *pOutMsgStream, Cluster_Message_Info *pstEncodeMsg)
{
    cUint32 uStreamIndex = 0;
    cUint32 uClusterCmdIndex = 0;

    if((NULL == pOutMsgStream) || (NULL == pstEncodeMsg))
    {
        CLUSTER_PRINT("Input param error!");
        return 0;
    }

    for(uClusterCmdIndex = 0; uClusterCmdIndex < CLUSTER_MAX_MSG_CONTAINER_NUM; uClusterCmdIndex++)
    {
        if(NULL != pstEncodeMsg->pastMessageCntnt[uClusterCmdIndex])
        {
            break;
        }
    }

    if(uClusterCmdIndex == CLUSTER_MAX_MSG_CONTAINER_NUM)
    {
        CLUSTER_PRINT("uClusterCmdIndex is max!");
        return 0;
    }

//    pOutMsgStream[uStreamIndex++] = (cUint8)(pstEncodeMsg->pastMessageBuf[uClusterCmdIndex]->stOperatorMessage.stMsgHeader.uSN >> 8);
//    pOutMsgStream[uStreamIndex++] = (cUint8)(pstEncodeMsg->pastMessageBuf[uClusterCmdIndex]->stOperatorMessage.stMsgHeader.uSN);
    uStreamIndex += 4;

    pOutMsgStream[uStreamIndex++] = (cUint8)(pstEncodeMsg->pastMessageCntnt[uClusterCmdIndex]->stControlMessage.stMsgHeader.uSendID >> 8);
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstEncodeMsg->pastMessageCntnt[uClusterCmdIndex]->stControlMessage.stMsgHeader.uSendID);

    pOutMsgStream[uStreamIndex++] = (cUint8)(pstEncodeMsg->pastMessageCntnt[uClusterCmdIndex]->stControlMessage.stMsgHeader.uMainNodeID >> 8);
    pOutMsgStream[uStreamIndex++] = (cUint8)(pstEncodeMsg->pastMessageCntnt[uClusterCmdIndex]->stControlMessage.stMsgHeader.uMainNodeID);

    for(; uClusterCmdIndex < CLUSTER_MAX_MSG_CONTAINER_NUM;uClusterCmdIndex++)
    {
        if(CLUSTER_MSG_TYPE_NONE != pstEncodeMsg->aenClusterMsgType[uClusterCmdIndex])
        {
            pOutMsgStream[uStreamIndex++] = (cUint8)pstEncodeMsg->aenClusterMsgType[uClusterCmdIndex];

            if(NULL == pstEncodeMsg->pastMessageCntnt[uClusterCmdIndex])
            {
                CLUSTER_PRINT("puMsgBuf is NULL!");
                return 0;
            }

            switch (pstEncodeMsg->aenClusterMsgType[uClusterCmdIndex])
            {
            case    CLUSTER_MSG_TYPE_STATE:
                {
                    fnCluster_codec_EncodeStateMsg(&pOutMsgStream[uStreamIndex], &pstEncodeMsg->pastMessageCntnt[uClusterCmdIndex]->stStateMessage, &uStreamIndex);
                }
            break;
            case    CLUSTER_MSG_TYPE_CONTROL:
                {
                    fnCluster_codec_EncodeControlMsg(&pOutMsgStream[uStreamIndex], &pstEncodeMsg->pastMessageCntnt[uClusterCmdIndex]->stControlMessage, &uStreamIndex);
                }
            break;
            default:
                {/*do Nothing*/}
            break;
            }
        }
    }

    if(uStreamIndex < 7)
    {
        CLUSTER_PRINT("Encode length  is too small!");
        return 0;
    }

    return uStreamIndex;
}
/*************************************cluster_codec.cpp****************************************/
/**************************************cluster_ctrl.cpp******************************************/
void fnCluster_ctrl_GpsPointToEnu(Cluster_Context *pstClusterContext,Cluster_Path_Buf *pstInPointArray,Cluster_Path_Buf *pstOutPointArray)
{
    for(cUint32 i = 0; i < pstInPointArray->uPathNum; i++)
    {
        /*基于单体当前位置计算出ScanArea的相对X，Y，Z*/
        CLUSTER_PRINT("Gps value is lon = %lf, lat = %lf!",pstInPointArray->aPathPointBuf[i].stPoint.x,pstInPointArray->aPathPointBuf[i].stPoint.y);
        fnCluster_codec_GpsConvertToEnu(pstOutPointArray->aPathPointBuf[i].stPoint.x,pstOutPointArray->aPathPointBuf[i].stPoint.y,
        pstInPointArray->aPathPointBuf[i].stPoint.x,pstInPointArray->aPathPointBuf[i].stPoint.y,
        pstClusterContext->astSubNodeCntxt[pstClusterContext->uCurrentID].stSubNodeGlobalPos.stPoint.x,
        pstClusterContext->astSubNodeCntxt[pstClusterContext->uCurrentID].stSubNodeGlobalPos.stPoint.y);
        CLUSTER_PRINT("Enu value is x = %lf, y = %lf!",pstOutPointArray->aPathPointBuf[i].stPoint.x,pstOutPointArray->aPathPointBuf[i].stPoint.y);

        /*将相对X，Y，Z矫正到基于起飞点的绝对X，Y，Z*/
        pstOutPointArray->aPathPointBuf[i].stPoint.x += pstClusterContext->astSubNodeCntxt[pstClusterContext->uCurrentID].stSubNodeInitialPos.stPoint.x;
        pstOutPointArray->aPathPointBuf[i].stPoint.x += pstClusterContext->astSubNodeCntxt[pstClusterContext->uCurrentID].stSubNodeOffsetPos.stPoint.x;
        pstOutPointArray->aPathPointBuf[i].stPoint.y += pstClusterContext->astSubNodeCntxt[pstClusterContext->uCurrentID].stSubNodeInitialPos.stPoint.y;
        pstOutPointArray->aPathPointBuf[i].stPoint.y += pstClusterContext->astSubNodeCntxt[pstClusterContext->uCurrentID].stSubNodeOffsetPos.stPoint.y;
        pstOutPointArray->aPathPointBuf[i].stPoint.z = pstInPointArray->aPathPointBuf[i].stPoint.z;
        CLUSTER_PRINT("Correct Enu value is x = %lf, y = %lf!",pstOutPointArray->aPathPointBuf[i].stPoint.x,pstOutPointArray->aPathPointBuf[i].stPoint.y);
    }
    pstOutPointArray->uPathNum = pstInPointArray->uPathNum;
}

void fnCluster_ctrl_AdjustScanArea(Cluster_Context *pstClusterContext)
{
    Cluster_Point *pstScanPoint = pstClusterContext->stOperatorCntxt.stScanArea.aPathPointBuf;

    if(0 == pstClusterContext->stOperatorCntxt.stScanArea.uPathNum)
    {
        /*路径最后一个点即为搜索区域，直接开检测,进入AutoWork*/
    }
    else if(1 == pstClusterContext->stOperatorCntxt.stScanArea.uPathNum)
    {
        /*与上面不同的是，没有将搜索点作为路径最后一个点，其他一致，所以也直接开检测*/
    }
    else if(4 == pstClusterContext->stOperatorCntxt.stScanArea.uPathNum)
    {
        /*顺序计算四个点与主节点的位置差，并将4个X点与4个Y点组成两个数组
        分别取{Xmin,Ymin},{Xmin,Ymax},{Xmax,Ymin},{Xmax,Ymax}成为新的四个点，也即X、Y轴对称的搜索长方形
        计算Xmax - Xmin作为AreaSideLen，Ymax - Ymin作为AreaSideWidth
        {(Xmax - Xmin)/2,(Ymax - Ymin)/2}作为搜索中心点*/
        cFloat64 xMin = 0xFFFFFFFF,yMin = 0xFFFFFFFF,xMax = 0,yMax = 0;
        cFloat64 xMinPositive = 0xFFFFFFFF,yMinPositive = 0xFFFFFFFF,xMaxPositive = 0,yMaxPositive = 0;
        for(cUint32 i = 0; i < pstClusterContext->stOperatorCntxt.stScanArea.uPathNum; i++)
        {
            if(fabs(pstScanPoint[i].stPoint.x) < xMinPositive)
            {
                xMinPositive = fabs(pstScanPoint[i].stPoint.x);
                xMin = pstScanPoint[i].stPoint.x;
            }
            if(fabs(pstScanPoint[i].stPoint.x) > xMaxPositive)
            {
                xMaxPositive = fabs(pstScanPoint[i].stPoint.x);
                xMax = pstScanPoint[i].stPoint.x;
            }
            if(fabs(pstScanPoint[i].stPoint.y) < yMinPositive)
            {
                yMinPositive = fabs(pstScanPoint[i].stPoint.y);
                yMin = pstScanPoint[i].stPoint.y;
            }
            if(fabs(pstScanPoint[i].stPoint.y) > yMaxPositive)
            {
                yMaxPositive = fabs(pstScanPoint[i].stPoint.y);
                yMax = pstScanPoint[i].stPoint.y;
            }
        }
        pstClusterContext->stOperatorCntxt.aMapScanInfo[0] = xMaxPositive - xMinPositive;
        pstClusterContext->stOperatorCntxt.aMapScanInfo[1] = yMaxPositive - yMinPositive;
        pstClusterContext->stOperatorCntxt.aMapScanInfo[2] = (xMax + xMin) / 2;
        pstClusterContext->stOperatorCntxt.aMapScanInfo[3] = (yMax + yMin) / 2;
        pstClusterContext->stOperatorCntxt.aMapScanInfo[4] = pstScanPoint[0].stPoint.z;
        pstClusterContext->stOperatorCntxt.aMapScanInfo[5] = pstClusterContext->uAvailableSubNodeNum;
    }
    else
    {
        /*打印异常，不允许三角形和线形搜索区域存在*/
    }
}
/**************************************cluster_ctrl.cpp******************************************/
/**************************************cluster_mh.cpp******************************************/
void fnCluster_mh_SaveControlMessage(Cluster_Context *pstClusterContext, Cluster_Control_Message *pstMainNodeMsg)
{
    cUint32 uSubNodeMapID = 0;
    Cluster_Path_Buf *pPathPointBuf = NULL;
    cUint32 uSendID = pstMainNodeMsg->stMsgHeader.uSendID;
    //cUint32 uSelectIDNum = pstMainNodeMsg->stMsgHeader.uSelectIDNum;
    //cUint32 auSelectID[CLUSTER_MAX_NODE_NUM];
    //memcpy(auSelectID, pstMainNodeMsg->stMsgHeader.auSelectID, pstMainNodeMsg->stMsgHeader.uSelectIDNum * sizeof(cUint32));

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_SUBNODE_MAP_ID))
    {
        if((pstClusterContext->uSubNodeNum != pstMainNodeMsg->uSubNodeNumber) && (pstMainNodeMsg->uSubNodeNumber != 0))
        {
            /*there has something wrong with network!*/
            CLUSTER_PRINT("there has something wrong with network!");
            pstClusterContext->uSubNodeNum = pstMainNodeMsg->uSubNodeNumber;
        }
        memcpy(pstClusterContext->auSubNodeMapID, pstMainNodeMsg->auSubNodeMapID, sizeof(cUint32) * pstClusterContext->uSubNodeNum);
    }

    if((0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PATH_POINT)) ||
        (0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PATH_POINT_GPS)))
    {
        pstClusterContext->bIsCommonPathUsed = false;
        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            for(cUint32 i = 0; i < pstClusterContext->uSubNodeNum; i++)
            {
                uSubNodeMapID = pstClusterContext->auSubNodeMapID[i];
                pPathPointBuf = pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID];
                pstClusterContext->astSubNodeCntxt[uSubNodeMapID].stSubNodePath.uPathNum = pPathPointBuf->uPathNum;
                if(pPathPointBuf->uPathNum != 0)
                {
                    memcpy(pstClusterContext->astSubNodeCntxt[uSubNodeMapID].stSubNodePath.aPathPointBuf, pPathPointBuf->aPathPointBuf, sizeof(Cluster_Point) * pPathPointBuf->uPathNum);
                }
            }
        }
        else if(CLUSTER_RECV_ID_ALL == pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            pstClusterContext->bIsCommonPathUsed = true;
            pPathPointBuf = pstMainNodeMsg->pastSubNodePathPoint[0];
            if(pPathPointBuf->uPathNum != 0)
            {
                if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PATH_POINT_GPS))
                {
                    CLUSTER_PRINT("Receive Control Msg Contain Path Point With GPS Formation!");
                    fnCluster_ctrl_GpsPointToEnu(pstClusterContext,pPathPointBuf,&pstClusterContext->stCommonPathPoint);
                }
                else
                {
                    pstClusterContext->stCommonPathPoint.uPathNum = pPathPointBuf->uPathNum;
                    memcpy(pstClusterContext->stCommonPathPoint.aPathPointBuf, pPathPointBuf->aPathPointBuf, sizeof(Cluster_Point) * pPathPointBuf->uPathNum);
                }
                pstClusterContext->stOperatorCntxt.stPathPoint.uPathNum = pstClusterContext->stCommonPathPoint.uPathNum;
                memcpy(pstClusterContext->stOperatorCntxt.stPathPoint.aPathPointBuf, pstClusterContext->stCommonPathPoint.aPathPointBuf, sizeof(Cluster_Point) * pstClusterContext->stCommonPathPoint.uPathNum);
            }
        }
        else
        {
            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                pPathPointBuf = pstMainNodeMsg->pastSubNodePathPoint[uSubNodeMapID];
                pstClusterContext->astSubNodeCntxt[uSubNodeMapID].stSubNodePath.uPathNum = pPathPointBuf->uPathNum;
                memcpy(pstClusterContext->astSubNodeCntxt[uSubNodeMapID].stSubNodePath.aPathPointBuf, pPathPointBuf->aPathPointBuf, sizeof(Cluster_Point) * pPathPointBuf->uPathNum);
                CLUSTER_PRINT("Receive Path Point in select ID %d!",pPathPointBuf->uPathNum);
                for(cUint32 j = 0; j < pPathPointBuf->uPathNum; j++)
                {
                    CLUSTER_PRINT("Path x = %lf, y = %lf!",pstClusterContext->astSubNodeCntxt[uSubNodeMapID].stSubNodePath.aPathPointBuf[j].stPoint.x,
                    pstClusterContext->astSubNodeCntxt[uSubNodeMapID].stSubNodePath.aPathPointBuf[j].stPoint.y);
                }
            }
        }
    }

    /*formation pos*/
    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_FORMATION_POS))
    {
        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            for(cUint32 i = 0; i < pstClusterContext->uSubNodeNum; i++)
            {
                uSubNodeMapID = pstClusterContext->auSubNodeMapID[i];
                memcpy(&pstClusterContext->astSubNodeCntxt[uSubNodeMapID].stFormationOffsetPos, &pstMainNodeMsg->astFormationPos[uSubNodeMapID],sizeof(Cluster_Point));
            }
        }
        else if(CLUSTER_RECV_ID_ALL != pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                memcpy(&pstClusterContext->astSubNodeCntxt[uSubNodeMapID].stFormationOffsetPos, &pstMainNodeMsg->astFormationPos[uSubNodeMapID],sizeof(Cluster_Point));
            }
        }
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_ASSIGN_TARGET_ID))
    {
        if(0 == pstMainNodeMsg->stMsgHeader.uSelectIDNum)
        {
            for(cUint32 i = 0; i < pstClusterContext->uSubNodeNum; i++)
            {
                uSubNodeMapID = pstClusterContext->auSubNodeMapID[i];
                pstClusterContext->astSubNodeCntxt[uSubNodeMapID].uSubNodeAssignTargetID = pstMainNodeMsg->auSubNodeAssignTargetID[uSubNodeMapID];
            }
        }
        else if(CLUSTER_RECV_ID_ALL != pstMainNodeMsg->stMsgHeader.auSelectID[0])
        {
            for(cUint32 i = 0; i < pstMainNodeMsg->stMsgHeader.uSelectIDNum; i++)
            {
                uSubNodeMapID = pstMainNodeMsg->stMsgHeader.auSelectID[i];
                pstClusterContext->astSubNodeCntxt[uSubNodeMapID].uSubNodeAssignTargetID = pstMainNodeMsg->auSubNodeAssignTargetID[uSubNodeMapID];
            }
        }
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_TARGET_POS_ALL))
    {
        pstClusterContext->uTargetAllNum = pstMainNodeMsg->stTargetGlobalPos.uPathNum;
        for(cUint32 i = 0; i < pstMainNodeMsg->stTargetGlobalPos.uPathNum; i++)
        {
            memcpy(&pstClusterContext->astTargetCntxt[i].stTargetGlobalPos, &pstMainNodeMsg->stTargetGlobalPos.aPathPointBuf[i], sizeof(Cluster_Point));
        }
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_COMMON_PATH_INDEX))
    {
        pstClusterContext->uCommonPathCurrentIndex = pstMainNodeMsg->uCurrExecPathPointIndex;
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_FORMATIPN_TYPE))
    {
        pstClusterContext->stOperatorCntxt.enFormationType = pstMainNodeMsg->enFormationType;
    }

    if((0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_AREA_INFO)) ||
        (0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_AREA_INFO_GPS)))
    {
        if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_AREA_INFO_GPS))
        {
            CLUSTER_PRINT("Receive Control Msg Contain Area Info With GPS Formation!");
            fnCluster_ctrl_GpsPointToEnu(pstClusterContext,&pstMainNodeMsg->stScanArea,&pstClusterContext->stOperatorCntxt.stScanArea);
        }
        else
        {
            memcpy(pstClusterContext->stOperatorCntxt.stScanArea.aPathPointBuf, pstMainNodeMsg->stScanArea.aPathPointBuf, sizeof(Cluster_Point) * pstMainNodeMsg->stScanArea.uPathNum);
        }
        fnCluster_ctrl_AdjustScanArea(pstClusterContext);
    }

    if((0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT)) ||
        (0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT_GPS)))
    {
        if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_PICKUP_POINT_GPS))
        {
            CLUSTER_PRINT("Receive Control Msg Contain Pickup Point With GPS Formation!");
            Cluster_Path_Buf stInPoint,stOutPoint;
            stInPoint.uPathNum = 1;
            memcpy(&stInPoint.aPathPointBuf, &pstMainNodeMsg->stPickupPoint, sizeof(Cluster_Point));
            fnCluster_ctrl_GpsPointToEnu(pstClusterContext,&stInPoint,&stOutPoint);
            memcpy(&pstClusterContext->stOperatorCntxt.stPickupPoint, stOutPoint.aPathPointBuf, sizeof(Cluster_Point));
        }
        else
        {
            memcpy(&pstClusterContext->stOperatorCntxt.stPickupPoint, &pstMainNodeMsg->stPickupPoint, sizeof(Cluster_Point));
        }
    }

    if(0 != (pstMainNodeMsg->uEncodeDirectField & CLUSTER_MAINNODE_MSG_IE_VELOCITY))
    {
        pstClusterContext->stOperatorCntxt.velocityLimit = pstMainNodeMsg->velocityLimit;
    }
}

void fnCluster_mh_SaveStateMessage(Cluster_Context *pstClusterContext, Cluster_State_Message *pstSubNodeMsg)
{
    cUint32 uSendID = pstSubNodeMsg->stMsgHeader.uSendID;
    cUint32 uCurrentID = pstClusterContext->uCurrentID;

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_INITIAL_POS))
    {
        memcpy(&pstClusterContext->astSubNodeCntxt[uSendID].stSubNodeInitialPos, &pstSubNodeMsg->stSubNodeInitialPos, sizeof(cFloat64) * 3);
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_OFFSET_POS))
    {
        memcpy(&pstClusterContext->astSubNodeCntxt[uSendID].stSubNodeOffsetPos, &pstSubNodeMsg->stSubNodeOffsetPos, sizeof(cFloat64) * 3);
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_GLOBAL_POS))
    {
        memcpy(&pstClusterContext->astSubNodeCntxt[uSendID].stSubNodeGlobalPos, &pstSubNodeMsg->stSubNodeGlobalPos, sizeof(cFloat64) * 3);
        fnCluster_codec_GpsConvertToEnu(pstClusterContext->astInterUavDistance[uSendID].stPoint.x, pstClusterContext->astInterUavDistance[uSendID].stPoint.y,
            pstClusterContext->astSubNodeCntxt[uSendID].stSubNodeGlobalPos.aArray[0], pstClusterContext->astSubNodeCntxt[uSendID].stSubNodeGlobalPos.aArray[1],
            pstClusterContext->astSubNodeCntxt[uCurrentID].stSubNodeGlobalPos.aArray[0], pstClusterContext->astSubNodeCntxt[uCurrentID].stSubNodeGlobalPos.aArray[1]);
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_VELOCITY))
    {
        memcpy(&pstClusterContext->astSubNodeCntxt[uSendID].stSubNodeVelocity, &pstSubNodeMsg->stSubNodeVelocity, sizeof(cFloat64) * 3);
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_NETWORK_STATE))
    {
        pstClusterContext->astSubNodeState[uSendID].enClusterNetworkState = pstSubNodeMsg->enClusterNetworkState;
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_SUBNODE_STATE))
    {
        pstClusterContext->astSubNodeState[uSendID].enClusterSubNodeState = pstSubNodeMsg->enClusterSubNodeState;
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_MISSION_STATE))
    {
        pstClusterContext->astSubNodeState[uSendID].enClusterMissionState = pstSubNodeMsg->enClusterMissionState;
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_WORK_STATE))
    {
        pstClusterContext->astSubNodeState[uSendID].enClusterWorkState = pstSubNodeMsg->enClusterWorkState;
    }

    if(0 != (pstSubNodeMsg->uEncodeDirectField & CLUSTER_SUBNODE_MSG_IE_TARGET_POS))
    {
        //fnCluster_if_TargetFusion(pstSubNodeMsg->uTargetNumber, pstSubNodeMsg->astTargetGlobalPos);
    }
}
/**************************************cluster_mh.cpp******************************************/

void fnCluster_mh_StructToArray(std_msgs::UInt8MultiArray &auMsgOut, cInt8* InMsgBuf, cUint32 uInMsgLen)
{
    //auMsgOut.data.resize(sizeof(uInMsgLen));
    for(cUint32 i = 0; i < uInMsgLen; i++){
        //auMsgOut.data[i] = InMsgBuf[i];
        auMsgOut.data.push_back(InMsgBuf[i]);
    }
}

bool fnCluster_mh_ArrayToStruct(std_msgs::UInt8MultiArrayConstPtr auInMsg, cInt8* OutMsgBuf, cUint32 ucheckMsgLen)
{
    //if((cUint32)auInMsg->data.size() != ucheckMsgLen)
    {
    //    return false;
    }

    for(cUint32 i = 0; i < ucheckMsgLen; i++)
    {
        OutMsgBuf[i] = auInMsg->data[i];
        //s_strLog.append(strTemp.sprintf("%x,",aMsgStream[i]));
    }

    return true;
}
